#!/usr/bin/env python3
"""
tf2问题诊断脚本
用于排查rf2o里程计在网络环境下的tf广播问题
"""

import subprocess
import time
import threading
from datetime import datetime

def run_command(cmd, timeout=10):
    """运行命令并返回结果"""
    try:
        result = subprocess.run(cmd, shell=True, capture_output=True, text=True, timeout=timeout)
        return result.returncode, result.stdout, result.stderr
    except subprocess.TimeoutExpired:
        return -1, "", "Command timeout"
    except Exception as e:
        return -1, "", str(e)

def check_tf_topics():
    """检查tf相关话题"""
    print("=" * 60)
    print("🔍 检查tf相关话题")
    print("=" * 60)
    
    # 检查/tf话题
    print("\n📡 检查/tf话题:")
    ret, out, err = run_command("ros2 topic list | grep tf")
    if ret == 0:
        print(out)
    else:
        print(f"❌ 错误: {err}")
    
    # 检查/tf话题信息
    print("\n📊 /tf话题详细信息:")
    ret, out, err = run_command("ros2 topic info /tf")
    if ret == 0:
        print(out)
    else:
        print(f"❌ 错误: {err}")
    
    # 检查/tf_static话题信息
    print("\n📊 /tf_static话题详细信息:")
    ret, out, err = run_command("ros2 topic info /tf_static")
    if ret == 0:
        print(out)
    else:
        print(f"❌ 错误: {err}")

def check_tf_tree():
    """检查tf树结构"""
    print("\n=" * 60)
    print("🌳 检查tf树结构")
    print("=" * 60)
    
    ret, out, err = run_command("ros2 run tf2_tools view_frames.py", timeout=15)
    if ret == 0:
        print("✅ tf树生成成功，请查看frames.pdf文件")
        print(out)
    else:
        print(f"❌ tf树生成失败: {err}")

def monitor_tf_data():
    """监控tf数据流"""
    print("\n=" * 60)
    print("📈 监控tf数据流 (10秒)")
    print("=" * 60)
    
    # 监控/tf话题数据
    print("\n🔄 监控/tf话题数据:")
    ret, out, err = run_command("timeout 10s ros2 topic echo /tf --once")
    if ret == 0:
        print("✅ 收到/tf数据:")
        print(out[:500] + "..." if len(out) > 500 else out)
    else:
        print(f"❌ 未收到/tf数据: {err}")
    
    # 检查tf数据频率
    print("\n📊 检查/tf数据频率:")
    ret, out, err = run_command("timeout 5s ros2 topic hz /tf")
    if ret == 0:
        print(out)
    else:
        print(f"❌ 无法获取频率信息: {err}")

def test_tf_echo_variations():
    """测试不同的tf_echo变体"""
    print("\n=" * 60)
    print("🧪 测试tf_echo变体")
    print("=" * 60)
    
    test_cases = [
        ("map", "base_link"),
        ("odom", "base_link"),
        ("base_link", "laser"),
    ]
    
    for parent, child in test_cases:
        print(f"\n🔍 测试 {parent} -> {child}:")
        ret, out, err = run_command(f"timeout 5s ros2 run tf2_ros tf2_echo {parent} {child}")
        if ret == 0 and out.strip():
            print(f"✅ 成功获取 {parent} -> {child} 变换")
            # 只显示前几行
            lines = out.strip().split('\n')[:10]
            print('\n'.join(lines))
        else:
            print(f"❌ 无法获取 {parent} -> {child} 变换")
            if err:
                print(f"错误信息: {err}")

def check_time_sync():
    """检查时间同步"""
    print("\n=" * 60)
    print("⏰ 检查系统时间")
    print("=" * 60)
    
    current_time = datetime.now()
    print(f"当前系统时间: {current_time}")
    
    # 检查ROS时间
    ret, out, err = run_command("ros2 param get /use_sim_time")
    if ret == 0:
        print(f"ROS时间参数: {out}")
    else:
        print("未找到ROS时间参数")

def check_network_connectivity():
    """检查网络连通性"""
    print("\n=" * 60)
    print("🌐 检查ROS网络连通性")
    print("=" * 60)
    
    # 检查节点列表
    print("\n📋 当前ROS节点:")
    ret, out, err = run_command("ros2 node list")
    if ret == 0:
        nodes = out.strip().split('\n')
        print(f"发现 {len(nodes)} 个节点:")
        for node in nodes:
            print(f"  - {node}")
    else:
        print(f"❌ 无法获取节点列表: {err}")
    
    # 检查是否有rf2o相关节点
    print("\n🤖 查找rf2o相关节点:")
    ret, out, err = run_command("ros2 node list | grep -i rf2o")
    if ret == 0 and out.strip():
        print(f"✅ 找到rf2o节点: {out}")
    else:
        print("❌ 未找到rf2o节点")

def main():
    """主函数"""
    print("🚀 开始tf2问题诊断")
    print(f"诊断时间: {datetime.now()}")
    
    try:
        # 1. 检查tf话题
        check_tf_topics()
        
        # 2. 检查网络连通性
        check_network_connectivity()
        
        # 3. 检查时间同步
        check_time_sync()
        
        # 4. 监控tf数据
        monitor_tf_data()
        
        # 5. 测试tf_echo变体
        test_tf_echo_variations()
        
        # 6. 检查tf树
        check_tf_tree()
        
    except KeyboardInterrupt:
        print("\n\n⚠️ 诊断被用户中断")
    except Exception as e:
        print(f"\n\n❌ 诊断过程中出现错误: {e}")
    
    print("\n" + "=" * 60)
    print("🏁 诊断完成")
    print("=" * 60)
    print("\n建议:")
    print("1. 如果/tf话题有数据但tf_echo无输出，可能是时间同步问题")
    print("2. 如果/tf话题无数据，检查rf2o节点是否正常运行")
    print("3. 如果只有特定frame无法获取，检查tf树结构")
    print("4. 考虑在远程设备上运行此脚本进行对比")

if __name__ == "__main__":
    main()